#ifndef _AMERICO_MATRIX22_H_
#define _AMERICO_MATRIX22_H_

#include <cmath>
#include "staticarray.h"
#include "Vector2.h"

// Matrix22 Declaration
template <typename TYPE>
class Matrix22
{
public:
	Matrix22();
	Matrix22(const Matrix22<TYPE> &aMatrix);
	~Matrix22();

	Matrix22 &operator=(const Matrix22 &aMatrix);

	bool operator==(const Matrix22 &aMatrix);
	bool operator!=(const Matrix22 &aMatrix);

	const TYPE &operator[](const unsigned int &anIndex) const;
	TYPE &operator[](const unsigned int &anIndex);

	Matrix22 operator+(const Matrix22 &aMatrix) const;
	Matrix22 operator-(const Matrix22 &aMatrix) const;
	Matrix22 operator*(const Matrix22 &aMatrix) const;

	Vector2<TYPE> operator*(const Vector2<TYPE> &aVector) const;

	Matrix22 &operator+=(const Matrix22 &aMatrix);
	Matrix22 &operator-=(const Matrix22 &aMatrix);
	Matrix22 &operator*=(const Matrix22 &aMatrix);

	Matrix22 operator~() const;

	void Init(const TYPE someData[4]);

	static Matrix22 CreateRotate(const TYPE &anAngleInRadians);

	static Matrix22 Identity();

private:
	Utils::StaticArray<TYPE, 2*2> myMatrix;
};

// Matrix22 Definition
template <typename TYPE>
Matrix22<TYPE>::Matrix22()
{
	myMatrix[0] = myMatrix[3] = 1;
	myMatrix[1] = myMatrix[2] = 0;
}

template <typename TYPE>
Matrix22<TYPE>::Matrix22(const Matrix22<TYPE> &aMatrix)
{
	myMatrix = aMatrix.myMatrix;
}

template <typename TYPE>
Matrix22<TYPE>::~Matrix22()
{
}

template <typename TYPE>
Matrix22<TYPE> &Matrix22<TYPE>::operator =(const Matrix22<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<4; ++index)
	{
		myMatrix[index] = aMatrix.myMatrix[index];
	}
	return *this;
}

template <typename TYPE>
bool Matrix22<TYPE>::operator ==(const Matrix22<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<4; ++index)
	{
		if(myMatrix[index] != aMatrix.myMatrix[index])
		{
			return false;
		}
	}
	return true;
}

template <typename TYPE>
bool Matrix22<TYPE>::operator !=(const Matrix22<TYPE> &aMatrix)
{
	return !(operator ==(aMatrix));
}

template <typename TYPE>
const TYPE &Matrix22<TYPE>::operator [](const unsigned int &anIndex) const
{
	return myMatrix[anIndex];
}

template <typename TYPE>
TYPE &Matrix22<TYPE>::operator [](const unsigned int &anIndex)
{
	return myMatrix[anIndex];
}

template <typename TYPE>
Matrix22<TYPE> Matrix22<TYPE>::operator +(const Matrix22<TYPE> &aMatrix) const
{
	Matrix22<TYPE> result(*this);
	result += aMatrix;
	return result;
}

template <typename TYPE>
Matrix22<TYPE> Matrix22<TYPE>::operator -(const Matrix22<TYPE> &aMatrix) const
{
	Matrix22<TYPE> result(*this);
	result -= aMatrix;
	return result;
}

template <typename TYPE>
Matrix22<TYPE> Matrix22<TYPE>::operator *(const Matrix22<TYPE> &aMatrix) const
{
	Matrix22<TYPE> result(*this);
	result *= aMatrix;
	return result;
}

template <typename TYPE>
Vector2<TYPE> Matrix22<TYPE>::operator *(const Vector2<TYPE> &aVector) const
{
	Vector2<TYPE> result(0,0);
	result.x = aVector.myX*myMatrix[0] + aVector.myY*myMatrix[2];
	result.y = aVector.myX*myMatrix[1] + aVector.myY*myMatrix[3];
	return result;
}

template <typename TYPE>
Matrix22<TYPE> &Matrix22<TYPE>::operator +=(const Matrix22<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<4; ++index)
	{
		myMatrix[index] += aMatrix.myMatrix[index];
	}
	return *this;
}

template <typename TYPE>
Matrix22<TYPE> &Matrix22<TYPE>::operator -=(const Matrix22<TYPE> &aMatrix)
{
	for(unsigned int index=0; index<4; ++index)
	{
		myMatrix[index] -= aMatrix.myMatrix[index];
	}
	return *this;
}

template <typename TYPE>
Matrix22<TYPE> Matrix22<TYPE>::operator~() const
{
	Matrix22<TYPE> result;
	for(unsigned int i=0; i<2; ++i)
	{
		for(unsigned int j=0; j<2; ++j)
		{
			result.myMatrix[i + j*2] = myMatrix[j + i*2];
		}
	}
	return result;
}

template <typename TYPE>
Matrix22<TYPE> &Matrix22<TYPE>::operator *=(const Matrix22<TYPE> &aMatrix)
{
	Matrix22<TYPE> matrix(*this);
	for(unsigned int i=0; i<2; ++i)
	{
		for(unsigned int j=0; j<2; ++j)
		{
			myMatrix[i + 2*j] = 0;
			for(unsigned int r=0; r<2; ++r)
			{
#ifdef MATRIX_COLUMN
				myMatrix[i + 2*j] += matrix.myMatrix[r + 2*i] * aMatrix.myMatrix[j + 2*r];
#else
				myMatrix[i + 2*j] += matrix.myMatrix[i + 2*r] * aMatrix.myMatrix[r + 2*j];
#endif
			}
		}
	}
	return *this;
}

template <typename TYPE>
void Matrix22<TYPE>::Init(const TYPE someData[9])
{
	for(unsigned int index=0; index<4; ++index)
	{
		myMatrix[index] = someData[index];
	}
}

template <typename TYPE>
Matrix22<TYPE> Matrix22<TYPE>::CreateRotate(const TYPE &anAngleInRadians)
{
	Matrix22<TYPE> result;
	result.myMatrix[0] = result.myMatrix[3] = cos(anAngleInRadians);
	result.myMatrix[2] = sin(anAngleInRadians);
	result.myMatrix[1] = -result.myMatrix[2];
	return result;
}

template <typename TYPE>
Matrix22<TYPE> Matrix22<TYPE>::Identity()
{
	return Matrix22<TYPE>();
}

#endif
